/*
 * StartApplication.java
 *
 * Created on October 8, 2006, 12:15 AM
 */

package org.sunspotworld;

import com.sun.spot.core.peripheral.IDriver;
import com.sun.spot.core.peripheral.Spot;
import com.sun.spot.core.resources.transducers.ITriColorLED;
import com.sun.spot.core.util.Utils;
import com.sun.spot.edemo.EDemoBoard;
import com.sun.spot.edemo.peripheral.Servo;
import javax.microedition.midlet.MIDlet;
import javax.microedition.midlet.MIDletStateChangeException;

/**
 * A Servo control demo application
 * We implement IDriver to tell the SPOT when it is ok to enter deep sleep and when it is not ok.
 * The SPOT has a power manager object that runs in the background and will attempt to enter the
 * least power consuming state it can when application threads allow it to.  
 * @author Douglas McClusky
 */
public class StartApplication extends MIDlet implements IDriver {
    private final int HALF_RANGE = 500; 
    private Servo[] wheels = {new Servo(EDemoBoard.getInstance().getOutputPins()[0]), new Servo(EDemoBoard.getInstance().getOutputPins()[1])};
    private int [] stop = new int [2];
    private ITriColorLED [] leds = EDemoBoard.getInstance ().getLEDs();
    private boolean servoActive = false;
    
    /**
     * Midlet execution starts here
     * @throws javax.microedition.midlet.MIDletStateChangeException
     */
    protected void startApp() throws MIDletStateChangeException {
        Spot.getInstance().getDriverRegistry().add(this);  //have the SPOT check with us before going to deep sleep
        stop[0] = 1463;  //the pulse width that holds servo 0 still (actual values will vary by servo)
        stop[1] = 1469;  //the pulse width that holds servo 1 still (actual values will vary by servo)
        for ( int i=0; i<2; i++) {
             wheels[i].setBounds(stop[i]-HALF_RANGE,stop[i]+HALF_RANGE);
        }
        doShape();
    }
    
    /**
     * hold the servos still for a given amount of time
     * @param ms - number of milliseconds to brake
     */
    private void brake(long ms) {
        servoActive = true;
        for(int i=0; i<2; i++) {
            wheels[i].setPosition((float).5);  //position .5 corresponds to the stop value above
        }
        Utils.sleep(ms);
        servoActive = false;
    }
    
    /**
     * Brake for a quarter second and then turn off the servos
     */
    private void stop() {
        brake(250);
        for(int i=0; i<2; i++) {
            wheels[i].setValue(0);  //set servo pins low
        }
    }
    
    /**
     * Go backward
     * Note: Since the servos are mounted opposite each other, the directions for one is the opposite of the direction of the other for a given Servo position.
     * @param ms - the number of milliseconds to go backward
     */
    private void reverse(long ms) {
        servoActive = true;
        leds[0].setRGB(50,0,0);
        leds[0].setOn();
        wheels[0].setPosition(1); 
        wheels[1].setPosition(0);
        Utils.sleep(ms);
        leds[0].setOff();
        servoActive = false;
    }
    
    /**
     * Go right
     * Note: Since the servos are mounted opposite each other, the directions for one is the opposite of the direction of the other for a given Servo position.
     * @param ms - the number of milliseconds to go right
     */
    private void right(long ms) {
        servoActive = true;
        leds[0].setRGB(0,50,0);
        leds[0].setOn();
        wheels[0].setPosition(0);
        wheels[1].setPosition(0);
        Utils.sleep(ms);
        leds[0].setOff();
        servoActive = false;
    }
    
    /**
     * Go left
     * Note: Since the servos are mounted opposite each other, the directions for one is the opposite of the direction of the other for a given Servo position.
     * @param ms - the number of milliseconds to go left
     */
    private void left(long ms) {
        servoActive = true;
        leds[0].setRGB(0,50,0);
        leds[0].setOn();
        wheels[0].setPosition(1);
        wheels[1].setPosition(1);
        Utils.sleep(ms);
        leds[0].setOff();
        servoActive = false;
    }
    
    /**
     * Go forward
     * Note: Since the servos are mounted opposite each other, the directions for one is the opposite of the direction of the other for a given Servo position.
     * @param ms - the number of milliseconds to go forward
     */
    private void forward(long ms) {
        servoActive = true;
        leds[0].setRGB(50,0,0);
        leds[0].setOn();
        wheels[0].setPosition(0); 
        wheels[1].setPosition(1);
        Utils.sleep(ms);
        leds[0].setOff();
        servoActive = false;
    }
    
    /**
     * execute a list of movement commands
     */
    private void doShape() {
        forward(3000);
        right(3000);
        left(3000);
        reverse(3000);
        stop();
    }
    
    //
    //IDriver methods
    //
    
    
    /**
     * Implements getDriverName of IDriver interface
     * @return the name of this Driver 
     */
    public String getDriverName() {
        return "ServoDemo";
    }
    
    /**
     * Check if robot is ready for SPOT to deep sleep
     * @return whether the servos need to sustain a pulse
     */
    public boolean tearDown() {
        return !servoActive;
    }

    /**
     * what to do when SPOT comes out of deep sleep
     */
    public void setUp() {
        //do nothing
    }
    
    /**
     * what to do when SPOT shuts down
     */
    public void shutDown() {
        //do nothing
    }
    
    //
    //MIDLet methods
    //

    protected void pauseApp() {
        // This will never be called by the Squawk VM
    }

    protected void destroyApp(boolean arg0) throws MIDletStateChangeException {
        // Only called if startApp throws any exception other than MIDletStateChangeException
    }
}
